#include "record.h"
#define IPI 0.0174532925199433333333        // pi/180
double last_lat = 0.0;
double last_lon = 0.0;
int total = 1;
int gfd;
FILE *fp;
FILE *fp1;
static int get_nextval(const char *T,int next[]);
static int DataProcess(char *buf,int len,FILE *fp);
static int ComInit(char *DevPath);
static int _write(int fd, void *buffer, int length);
//char temp[48] = {"/home/jl/JL/record/track_%s.txt"};
//char temp1[48] = {"/home/jl/JL/record/ori_%s.txt"};
char path[48] = { 0 };
char path1[48] = { 0 };

int RtkSet(char *path)		// 设置RTK参数
{
    int fd;
    int res = 0;
    char buffer[128] = {"$GPGGA,002048.000,3149.0592,N,12000.8162,E,1,9,0.93,14.1,M,6.7,M,,*5B"};
    char end[2] = {0x0d,0x0a};
    fd = ComInit(path);
    if(fd == -1)
        return fd;
    else
    {
         res = _write(fd, buffer, strlen(buffer));
         res = _write(fd, end, 2);
         close(fd);
    }
    return res;
}

int IsEqual(double a,double b)
{
    return fabs(a-b) < 0.000001;
}

int kmp(const char *Text,const char* Pattern)
{
    if(!Text || !Pattern || Pattern[0] == '\0' || Text[0]=='\0')
        return -1;
    int llen=0;
    const char* c = Pattern;
    while(*c++ != '\0')
        ++llen;
    int* next = (int*)malloc(sizeof(int) * (llen + 1));
    // int*  next = new int[len+1];
    get_nextval(Pattern,next);
    int index ,i ,j;
    i = j = index = 0;
    while(Text[i]!='\0' && Pattern[j]!='\0')
    {
        if(Text[i] == Pattern[j])
        {
            ++i;
            ++j;
        }
        else
        {
            index += j-next[j];
            if(next[j] != -1)
                j = next[j];
            else
            {
                j = 0;
                ++i;
            }
        }
    }
    free (next);
    if(Pattern[j] == '\0')
        return index;
    else
        return -1;
}

int get_nextval(const char *T,int next[])
{
    int j = 0, k = -1;
    next[0] = -1;
    while (T[j] != '\0')
    {
        if (k == -1 || T[j] == T[k])
        {
            ++j;
            ++k;
            if(T[j] != T[k])
                next[j] = k;
            else
                next[j] = next[k];
        }
        else
            k = next[k];
    }
    return 0;
}

int _write(int fd, void *buffer, int length)
{
    int bytes_left;
    int written_bytes;
    char *ptr;

    ptr = buffer;
    bytes_left = length;
    while (bytes_left > 0)
    {
        /********开始写 ***********/
        written_bytes=write(fd, ptr, bytes_left);
        if (written_bytes <= 0) // 出错了
        {
            if (errno==EINTR) // 中断错误继续写
                written_bytes = 0;
            else
                // 其他错误退出
                return (-1);
        }
        bytes_left -= written_bytes;
        ptr += written_bytes; // 从剩下的地方继续写
    }
    return (0);
}

// lat->x   lon->y
void WGS84ToGuass(double latitude,double longitude,double *x,double *y)
{
    double p =206264.8062470963;     //p表示206265
    double B;  //纬度
    double L;  //经度
    double L0;  //中央经度
    double c = 6399593.625803977;
    double e2 = 0.006739496742227;
    double c0 = 6367449.145928;
    double c2 = -32144.479876;
    double c4 = 135.366946;
    double c6 = -0.709481;
    double c8 = 0.004061;
    double N,l,t2,n2,l0;    // n2是η的平方
    double a1,a2,b1,b2,dx,dy,cx,cy;     // r为该点的平面子午线收敛角
    double X;               // X是子午线弧长
    double zonewide = 3;
    double prjno;

    L0 = longitude;
    if (zonewide == 6)
    {
        prjno = trunc(L / zonewide) + 1;
        L0 = prjno * zonewide - 3;
    }
    else
    {
        prjno = trunc((L0 - 1.5) / 3) + 1;
        L0 = prjno * 3;
    }
    B = latitude;
    L = longitude;
    l = (L-L0)*3600.0;      // 经度秒差
    B = B*IPI;              // 纬度纯度转换为弧度
    double tcos = cos(B);
    double tsin = sin(B);
    double ttan = tan(B);
    N = c/sqrt(1+e2*pow(tcos,2));
    X = c0*B+(c2*tcos+c4*pow(tcos,3)+c6*pow(tcos,5)+c8*pow(tcos,7))*tsin;
    t2 = pow(ttan,2);       // t的平方
    n2 = e2*pow(tcos,2);
    l0 = l*l*0.00000001;
    a1 = N*tsin*tcos*0.0011752215269548;
    a2 = N*tsin*pow(tcos,3)*(5-t2+9*n2+4*n2*n2)*0.00000023019093957;
    b1 = N*tcos/p;
    b2 = N*pow(tcos,3)*(1-t2+n2)*0.000000001899211582;
    dx = N*tsin*pow(tcos,5)*(61-58*t2+t2*t2)*pow(l,6)/(720*pow(p,6));
    dy = N*pow(tcos,5)*(5-18*t2+t2*t2+14*n2-58*n2*t2)*pow(l,5)/(120*pow(p,5));
    cx = X+l0*(a1+a2*l0)+dx;
    cy = l*(b1+b2*l0)+dy;
    *y = cx;
    *x = cy+500000;
    if(latitude == 90)
        *x = 500000;
}

void GpsProcess(void)
{
    unsigned char temp;
    int count,start;
    fd_set readset; // 设置文件描述符集
    struct timeval tv;
    int MaxFd,nFd; // 文件描述符个数
    char buf[512] = {0};
    start = count = 0;
    do{
        FD_ZERO( &readset); // 文件描述符置0
        if (gfd >= 0)
            FD_SET(gfd, &readset);
        MaxFd = gfd + 1; // 最大文件描述符数
        tv.tv_sec = 4;
        tv.tv_usec = 0;
        do {
            nFd = select(MaxFd, &readset, 0, 0, &tv); // 检测状态
        } while (nFd==-1 && errno==EINTR); // 是否中断
        if (nFd <= 0)
            gfd = -1;
        else if(FD_ISSET(gfd, &readset)) // FD_ISSET检测fd是否设置
        {
            if(read(gfd, &temp, 1)>0)
            {
                if(temp == '$')			// pack head
                    start = 1;
                else if(temp == 0x0A)
                {
                     // _gps_printf("buf: %s\n",buf);
                    if(DataProcess(buf,count,fp) == 0)
                        break;
                    else
                        memset(buf,0x00,count);
                    count = start = 0;
                }
                if(start)
                    buf[count++] = temp;
            }
            else
                break;
        }
    }while(gfd >= 0);
    printf("GPS read error!\n");
}

/***@brief  设置串口通信速率
 *@param  fd     类型 int  打开串口的文件句柄
 *@param  speed  类型 int  串口速度
 *@return  void
 */
#define BAUDRATE B115200
int GPSComInit(char *DevPath)
{
    int fd;
    struct termios newtio;
    fd = open(DevPath, O_RDWR|O_NOCTTY|O_NDELAY);
    if (fd == -1)
    {
        printf("can't open GPS port!\n");
        return (-1);
    }
    tcgetattr(fd,&newtio);
    bzero(&newtio,sizeof(newtio));
    //setting c_cflag
    newtio.c_cflag &= ~CSIZE;
    newtio.c_cflag |= (CLOCAL|CREAD);
    newtio.c_cflag &= ~PARENB;
    newtio.c_cflag &= ~PARODD;
    newtio.c_cflag &= ~CSTOPB;
    newtio.c_cflag |= CS8;
    newtio.c_oflag |= OPOST;
    newtio.c_oflag &= ~ONLCR;
    newtio.c_cflag &= ~HUPCL;
    newtio.c_lflag &= ~IEXTEN;
    newtio.c_lflag &= ~ECHOK;
    newtio.c_lflag &= ~ECHOCTL;
    newtio.c_lflag &= ~ECHOKE;
    newtio.c_iflag &= ~INPCK;
    newtio.c_iflag |= IGNBRK;
    newtio.c_iflag &= ~ICRNL;
    newtio.c_iflag &=~ (IXON|IXOFF|IXANY);
    cfsetispeed(&newtio,BAUDRATE);
    cfsetospeed(&newtio,BAUDRATE);
    //printf("speed=%d\n",baud);
    tcflush(fd, TCIFLUSH);
    tcsetattr(fd,TCSANOW,&newtio);
    return(fd);
}

int ComInit(char *DevPath)
{
    int fd;
    struct termios newtio;
    fd = open(DevPath, O_RDWR|O_NOCTTY|O_NDELAY);
    if (fd == -1)
    {
        printf("Com: Can't open!\n");
        return (-1);
    }
    tcgetattr(fd,&newtio);
    bzero(&newtio,sizeof(newtio));
    //setting c_cflag
    newtio.c_cflag |= (CLOCAL|CREAD);
    newtio.c_cflag &= ~PARENB;
    newtio.c_cflag &= ~PARODD;
    newtio.c_cflag &= ~CSTOPB;
    newtio.c_cflag &= ~CSIZE;
    newtio.c_cflag |= CS8;
    newtio.c_oflag |= OPOST;
    //newtio.c_lflag = 0;
    //newtio.c_cc[VTIME] = 5;
    //newtio.c_cc[VMIN] = 0;
    //setting c_iflag
    newtio.c_iflag &=~ (IXON|IXOFF|IXANY);
    cfsetispeed(&newtio,BAUDRATE);
    cfsetospeed(&newtio,BAUDRATE);
    //printf("speed=%d\n",baud);
    tcflush(fd, TCIFLUSH);
    tcsetattr(fd,TCSANOW,&newtio);
    return(fd);
}


int DataProcess(char *buf,int len,FILE *fp)		// 解析GPS数据
{
    char yaw[8] = {0};
    char latitude[16] = {0};
    double lat=0.0,lon=0.0;
    char longitude[16] = {0};
    char speed[8] = {0};
    int index = 0,i,status=0;
	double x,y;
    char temp[16] = {0};
    char str[128] = {0};
	char str1[128] = {0};
	printf("%s\n",buf);
    if(len>23 && (kmp(buf,"$GPFPD")==0))
    {
        buf += 7;
        for(i=0;;i++)
        {
            index=kmp(buf,",");
            if(index <= 0)
                break;
            memcpy(temp,buf,index);

            switch (i)
            {
            case 2:                                 //偏航角(hhh.hh)
                memcpy(yaw,temp,index);
                break;
            case 5:         //纬度(+/-ll.lllllll)(‘+’-北纬，‘-’-南纬)
                memcpy(latitude,temp,index);
                lat = strtod(temp,NULL);
                break;
            case 6:         //经度(+/-lll.lllllll)(‘+’-东经，‘-’-西经)
                memcpy(longitude,temp,index);
                lon = strtod(temp,NULL);
                break;
            case 9:                //north速度(+/-eee.eee) 单位：米/秒
                memcpy(speed,temp,index);
                break;
            case 11:                       //基线长度(bb.bbb) 单位：米
       //         printf("base line lenght: %s\n",temp);
                break;
            default:
                break;
            }
            memset(temp,0x00,index);
            buf += (index+1);
        }
        buf++;
        if(*buf > 64)
            status = (*buf)-55;
        else
            status = (*buf)-48;

    }
    if(status == 5 || status == 0x0B)        // RTK标志,置位则表示进入了RTK状态,可以开始录数据
    {
        if(!IsEqual(lat,last_lat) || !IsEqual(lon,last_lon))
        {
            WGS84ToGuass(strtod(latitude,NULL),strtod(longitude,NULL),&x,&y);			/// WGS-84坐标转高斯坐标
            sprintf(str,"%lf %lf %s %s %s %s\n",x,y,yaw,speed,latitude,longitude);		// 合成要写入的数据,speed无用
            sprintf(str1,"%lf %lf\n",x,y);
            printf("str: %s\n",str);
            fputs(str,fp);				// 写入地图文件
            fputs(str1,fp1);			// 写入纯坐标文件
        }
        last_lat = lat;
        last_lon = lon;
    }
    return 1;
}

void stop(int a)
{
    printf("out: %d !\n",a);
    fclose(fp);
	fclose(fp1);
    close(gfd);
    exit(0);
}

int main(int argc, char ** argv)
{
	char *env= "/home/jl/JL/map/";
	if(path == NULL)
	{
		printf("Please check the environment variable!\n");
		exit(0);
	}
	if(argc != 3)
	{
		printf("Usage: ./record (track_name) (coor_name)\n");
		exit(0);
	}
	else
	{
		sprintf(path,"%s%s",env,argv[1]);		// 地图文件路径
		sprintf(path1,"%s%s",env,argv[2]);		// 纯坐标文件路径
		printf("track file path: %s\n",path);
		printf("coor file path: %s\n",path1);
	}

    signal(SIGINT, stop);		// ctrl+C
    fp1 = fopen(path1, "w+");
    if((fp=fopen(path, "w+")) == NULL)
    {
        printf("can't open file!\n");
        return 0;
    }
    if((gfd = GPSComInit("/dev/ttyS0")) == -1)	// GPS数据获取串口
    {
        stop(-1);
        exit (0);
    }
    else
        ;//printf("gfd: %d\n",gfd);
    if(RtkSet("/dev/ttyS1") == -1)				// 从RTK设置串口写入RTK设置数据
        printf("GPS: RTK setting failed!\n");
    GpsProcess();
    fclose(fp);
	fclose(fp1);
	exit(0);
//    fclose(fp1);
}
